#include <Servo.h>
#include <KK2LCD.h>

// Timelapse on KK2 code by Ajay Patadia is licensed under a Creative Commons Attribution 3.0 Unported License.
//http://creativecommons.org/licenses/by/3.0/deed.en_US

//I have used some portion of the test code by Marc Griffith is licensed under a Creative Commons Attribution 3.0 Unported License.
//http://creativecommons.org/licenses/by/3.0/deed.en_US

//instructions and videos on getting KK2 working with Arduino are available here
//http://www.rchacker.com/diy/arduino-on-the-kk2-multicopter-controller


//AIL, THR etc
//can also be digital outputs

const byte IN1 = 0;  //PD3 (PCINT27/TXD1/INT1)  not tested, but use Serial1 
const byte IN2 = 1;  //PD2 (PCINT26/RXD1/INT0)  interrupts good for CCPM decoding.
const byte IN3 = 2;  //PD0 (PCINT24/RXD0/T3)  tx0 is on the lcd not sure if using this would conflict with the lcd  
const byte IN4 = 3;  //PB2 (PCINT10/INT2/AIN0)
const byte IN5 = 4;  //PB0 (PCINT8/XCK0/T0)  //timer/counter0 source

//motor outputs can also be digital inputs. these also have PCINT16 to 23 Arduino interrupts not tested.
const byte OUT1 = 5;  //PC6 (TOSC1/PCINT22)   //32.768kHz crystal or custom clock source for counter (rpm sensor)
const byte OUT2 = 6;  //PC4 (TDO/PCINT20)   //JTAG 
const byte OUT3 = 7;  //PC2 (TCK/PCINT18)   //JTAG
const byte OUT4 = 8;  //PC3 (TMS/PCINT19)  //JTAG
const byte OUT5 = 9;  //PC1 (SDA/PCINT17)  //I2C      i2c not tested
const byte OUT6 = 10; //PC0 (SCL/PCINT16)  //I2C
const byte OUT7 = 11; //PC5 (TDI/PCINT21)   //JTAG
const byte OUT8 = 12; //PC7 (TOSC2/PCINT23)   //32.768kHz crystal

const byte RED_LED = 13;  //PB3 (PCINT11/OC0A/AIN1)  //same as arduino!

//important enable the internal pullups when using these as inputs
const byte BUT1 = 14;  //PB7 (PCINT15/OC3B/SCK)    PWM     pwm not tested
const byte BUT2 = 15;  //PB6 (PCINT14/OC3A/MISO)   PWM
const byte BUT3 = 16;  //PB5 (PCINT13/ICP3/MOSI)
const byte BUT4 = 17;  //PB4 (PCINT12/OC0B/SS)

const byte _BUZZER = 18;  //PB1 (PCINT9/CLKO/T1)   CLOCK output can adjust with system prescaler. (make tones) not tested

//analog reads must be done using thier channels, specifying digital pin numbers will not work in this case
const byte BATT = 3;

const byte GYR_R = 1;
const byte GYR_Y = 2;
const byte GYR_P = 4;

const byte ACC_X = 5;
const byte ACC_Y = 6;
const byte ACC_Z = 7;

//most of the hardware pwm is on the LCD, LED pins so dont bother. There is PWM on the buttons.

const int switch_release_debounce_us = 1000; //milliseconds
const int switch_press_debounce_uS = 1000; //microseconds

Servo myservo;

int zeropos = 73;
int dir = 0;
int time = 0;
int dist = 0;
int interval = 0;
String Str;
char str[7];

void setup() {
  pinMode(RED_LED, OUTPUT); 
  pinMode(GYR_R, INPUT);
  pinMode(GYR_Y, INPUT);
  pinMode(GYR_P, INPUT);
  pinMode(ACC_X, INPUT);
  pinMode(ACC_Y, INPUT);
  pinMode(ACC_Z, INPUT);

  pinMode(BUT1,INPUT);
  digitalWrite(BUT1, HIGH);   //enable internal pullup.

  pinMode(BUT2,INPUT);
  digitalWrite(BUT2, HIGH);

  pinMode(BUT3,INPUT);
  digitalWrite(BUT3, HIGH);

  pinMode(BUT4,INPUT);
  digitalWrite(BUT4, HIGH);

  analogReference(EXTERNAL); //important!!

  st7565Init( Font5x7 );
  st7565SetBrightness(12);
  st7565DrawString_P( 64, 40, PSTR("Time-Lapse") );
  st7565DrawString_P( 64, 32, PSTR(" the KK2.0") );  
  st7565DrawString_P( 64, 24, PSTR("   v1.0") );  
  st7565DrawString_P( 64, 16, PSTR("   Ajay") );
  st7565DrawString_P( 50, 1, PSTR("Press any key") );
  st7565WriteLogo(); //see library to modify
  
  myservo.attach(OUT1);  // attaches the servo on pin 9 to the servo object
  
  delay(2000);
  while(true)
  {
    if(!digitalRead(BUT1)||!digitalRead(BUT2)||!digitalRead(BUT3)||!digitalRead(BUT4)) {
      break;
    }
  }
}

byte buttonPressed(byte but)
{
  if(!digitalRead(but))
  {
    delayMicroseconds(switch_press_debounce_uS);
    if(!digitalRead(but))
    {
      while(!digitalRead(but))
      {
        digitalWrite(RED_LED,HIGH);
      }
      delayMicroseconds(switch_release_debounce_us);
      digitalWrite(RED_LED,LOW);    
      return 1;
    }
  }
  return 0;
}



// first page. The board does not do anything on this screen
void landing()
{
  while(true)
  {
    st7565ClearBuffer();
    st7565SetBrightness(12);
    st7565SetFont( Font12x16 );
    st7565DrawString_P( 0, 0,  PSTR("Time Lapse") );
    st7565SetFont( Font5x7 );
    st7565DrawString_P( 0, 15, PSTR("Intervalometer") ); 
    st7565DrawString_P( 0, 25, PSTR("v1.0") );  
    st7565DrawString_P( 0, 35, PSTR("Ajay") );
    st7565DrawString_P( 0, 54, PSTR("Start") );
    st7565DrawString_P( 98, 54, PSTR("Setup") );
    st7565Refresh();     
    if(buttonPressed(BUT1))
    { 
      confirm_start();
    }
    if(buttonPressed(BUT4))
    { 
      setup1();
    }
  }  
}

void confirm_start()
{
  while(true)
  {
    st7565ClearBuffer();
    st7565SetBrightness(12);
    st7565SetFont( Font12x16 );
    st7565DrawString_P( 0, 0,  PSTR("Sure ?") );
    st7565SetFont( Font5x7 );
    st7565DrawString_P( 0, 25, PSTR("Distant:") );  
    Str = String(dist);    
    Str.toCharArray(str,4); 
    st7565DrawString(50,25,str);
    st7565DrawString_P( 70, 25, PSTR("mm's") );
    st7565DrawString_P( 0, 35, PSTR("Interval: ") );
    Str = String(interval);    
    Str.toCharArray(str,4); 
    st7565DrawString(60,35,str);
    st7565DrawString_P( 75, 35, PSTR(" Sec's") ); 
    st7565DrawString_P( 0, 54, PSTR("YES") );
    st7565DrawString_P( 115, 54, PSTR("NO") );
    st7565Refresh();
    if(buttonPressed(BUT1))
    { 
      running();
    }
    if(buttonPressed(BUT4))
    { 
      landing();
    }
  }
}

void running()
{
  while(true)
  {
    st7565ClearBuffer();
    st7565SetBrightness(12);
    st7565SetFont( Font12x16 );
    st7565DrawString_P( 0, 0,  PSTR("Running...") );
    st7565SetFont( Font5x7 );
    st7565DrawString_P( 0, 15, PSTR("Total Time: 00:00:00") );  
    st7565DrawString_P( 0, 25, PSTR("Distant:") );  
    Str = String(dist);    
    Str.toCharArray(str,4); 
    st7565DrawString(50,25,str);
    st7565DrawString_P( 70, 25, PSTR("mm's") );
    st7565DrawString_P( 0, 35, PSTR("Interval: ") );
    Str = String(interval);    
    Str.toCharArray(str,4); 
    st7565DrawString(60,35,str);
    st7565DrawString_P( 75, 35, PSTR(" Sec's") );
    st7565DrawString_P( 0, 45, PSTR("Direction:") );
    if(dir==0)
    {  
      st7565DrawString_P( 75, 45, PSTR("CW") );
    }else
    {
      st7565DrawString_P( 75, 45, PSTR("CCW") );
    }
    st7565DrawString_P( 0, 54, PSTR("Stop") );
    st7565DrawString_P( 100, 54, PSTR("Pause") );
    st7565Refresh();
  
    
    myservo.write(dir); 
    
    delay(400);
        
    myservo.write(zeropos);     
    delay(interval*1000);
    
    if(buttonPressed(BUT4))
    { 
      paused();
    }
    if(buttonPressed(BUT1))
    { 
      landing();
    }
  }
}

void paused()
{
  while(true)
  {
    st7565ClearBuffer();
    st7565SetBrightness(12);
    st7565SetFont( Font12x16 );
    st7565DrawString_P( 0, 0,  PSTR("Paused...") );
    st7565SetFont( Font5x7 );
    st7565DrawString_P( 0, 15, PSTR("Total Time: 00:00:00") );
    st7565DrawString_P( 0, 25, PSTR("Distant:") );  
    Str = String(dist);    
    Str.toCharArray(str,4); 
    st7565DrawString(50,25,str);
    st7565DrawString_P( 70, 25, PSTR("mm's") );
    st7565DrawString_P( 0, 35, PSTR("Interval: ") );
    Str = String(interval);    
    Str.toCharArray(str,4); 
    st7565DrawString(60,35,str);
    st7565DrawString_P( 75, 35, PSTR(" Sec's") );
    st7565DrawString_P( 0, 45, PSTR("Direction:") );
    if(dir==0)
    {  
       st7565DrawString_P( 75, 45, PSTR("CW") );
    }else
    {
       st7565DrawString_P( 75, 45, PSTR("CCW") );
    }
    st7565DrawString_P( 0, 54, PSTR("Stop") );
    st7565DrawString_P( 95, 54, PSTR("Resume") );
    st7565Refresh();  
    if(buttonPressed(BUT4))
    { 
      running();
    }
    if(buttonPressed(BUT1))
    { 
      landing();
    }
  }
}

void setup1()
{
  while(true)
  {
    st7565ClearBuffer();
    st7565SetBrightness(12);
    st7565SetFont( Font12x16 );
    st7565DrawString_P( 0, 0,  PSTR("Setup 1") );
    st7565SetFont( Font5x7 );
    st7565DrawString_P( 0, 20, PSTR("Interval: ") );
    Str = String(interval);    
    Str.toCharArray(str,4); 
    st7565DrawString(60,20,str);
    st7565DrawString_P( 75, 20, PSTR(" Sec's") );
    st7565DrawString_P( 0, 54, PSTR("Back") );
    st7565DrawString_P( 45, 54, PSTR("-") );
    st7565DrawString_P( 75, 54, PSTR("+") );
    st7565DrawString_P( 105, 54, PSTR("Next") );
    st7565Refresh();
    if(buttonPressed(BUT4))
    { 
      setup2();
    }
    if(buttonPressed(BUT3))
    { 
      interval=interval+1;
    }
    if(buttonPressed(BUT2))
    { 
      interval=interval-1;
    }
    if(buttonPressed(BUT1))
    { 
      landing();
    }
  }
}

void setup2()
{
  while(true)
  {
    st7565ClearBuffer();
    st7565SetBrightness(12);
    st7565SetFont( Font12x16 );
    st7565DrawString_P( 0, 0,  PSTR("Setup 2") );
    st7565SetFont( Font5x7 );
    st7565DrawString_P( 0, 20, PSTR("Distant:") );  
    Str = String(dist);    
    Str.toCharArray(str,4); 
    st7565DrawString(50,20,str);
    st7565DrawString_P( 70, 20, PSTR("mm's") );
    st7565DrawString_P( 0, 54, PSTR("Back") );
    st7565DrawString_P( 45, 54, PSTR("-") );
    st7565DrawString_P( 75, 54, PSTR("+") );
    st7565DrawString_P( 105, 54, PSTR("Next") );
    st7565Refresh();  
    if(buttonPressed(BUT4))
    { 
      setup3();
    }
    if(buttonPressed(BUT3))
    { 
      dist=dist+1;
    }
    if(buttonPressed(BUT2))
    { 
      dist=dist-1;
    }
    if(buttonPressed(BUT1))
    { 
      setup1();
    }
  }
}

void setup3()
{
  while(true)
  {
    st7565ClearBuffer();
    st7565SetBrightness(12);
    st7565SetFont( Font12x16 );
    st7565DrawString_P( 0, 0,  PSTR("Setup 3") );
    st7565SetFont( Font5x7 );
    st7565DrawString_P( 0, 20, PSTR("Direction:") );  
    if(dir==0)
    {  
       st7565DrawString_P( 70, 20, PSTR("CW") );
    }else
    {
       st7565DrawString_P( 70, 20, PSTR("CCW") );
    }
    st7565DrawString_P( 0, 54, PSTR("Back") );
    st7565DrawString_P( 45, 54, PSTR("CCW") );
    st7565DrawString_P( 75, 54, PSTR("CW") );
    st7565DrawString_P( 105, 54, PSTR("Next") );
    st7565Refresh();  
    if(buttonPressed(BUT4))
    { 
      setup4();
    }
    if(buttonPressed(BUT3))
    { 
      dir=0;
      st7565ClearBuffer();
      st7565SetBrightness(12);
      st7565SetFont( Font12x16 );
      st7565DrawString_P( 0, 0,  PSTR("Setup 3") );
      st7565SetFont( Font5x7 );
      st7565DrawString_P( 0, 20, PSTR("Direction:") );  
      st7565DrawString_P( 70, 20, PSTR("CW") );
      st7565DrawString_P( 0, 54, PSTR("Back") );
      st7565DrawString_P( 45, 54, PSTR("CCW") );
      st7565DrawString_P( 75, 54, PSTR("CW") );
      st7565DrawString_P( 105, 54, PSTR("Next") );
      st7565Refresh();  
    }
    if(buttonPressed(BUT2))
    { 
      dir=170;
      st7565ClearBuffer();
      st7565SetBrightness(12);
      st7565SetFont( Font12x16 );
      st7565DrawString_P( 0, 0,  PSTR("Setup 3") );
      st7565SetFont( Font5x7 );
      st7565DrawString_P( 0, 20, PSTR("Direction:") );  
      st7565DrawString_P( 70, 20, PSTR("CCW") );
      st7565DrawString_P( 0, 54, PSTR("Back") );
      st7565DrawString_P( 45, 54, PSTR("CCW") );
      st7565DrawString_P( 75, 54, PSTR("CW") );
      st7565DrawString_P( 105, 54, PSTR("Next") );
      st7565Refresh();  
   }
    if(buttonPressed(BUT1))
    { 
      setup2();
    }
  }
}

void setup4()
{
  zeropos=0;
  while(true)
  {
    st7565ClearBuffer();
    st7565SetBrightness(12);
    st7565SetFont( Font12x16 );
    st7565DrawString_P( 0, 0,  PSTR("Setup 4") );
    st7565SetFont( Font5x7 );
    st7565DrawString_P( 0, 20, PSTR("Zero Position:") );  
    Str = String(zeropos);    
    Str.toCharArray(str,4); 
    st7565DrawString(85,20,str);
    st7565DrawString_P( 105, 20, PSTR("Deg") );
    st7565DrawString_P( 0, 54, PSTR("Back") );
    st7565DrawString_P( 45, 54, PSTR("+3") );
    st7565DrawString_P( 75, 54, PSTR("+1") );
    st7565DrawString_P( 105, 54, PSTR("Next") );
    st7565Refresh();  
    if(buttonPressed(BUT4))
    { 
      confirm_start();
    }
    if(buttonPressed(BUT3))
    { 
      zeropos=zeropos+1;
      myservo.write(zeropos);
    }
    if(buttonPressed(BUT2))
    { 
      zeropos=zeropos+3;
      myservo.write(zeropos);
    }
    if(buttonPressed(BUT1))
    { 
      setup3();
    }
  }
}




void loop() {
  landing();
}


